/**
 * @(#) NXTRobotController.java
 */

package pcsr.drivers.nxtDriver;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Hashtable;
import java.util.LinkedList;

import pcsr.hwVirtualization.Actuator;
import pcsr.hwVirtualization.ActuatorMode;
import pcsr.hwVirtualization.ActuatorValue;
import pcsr.hwVirtualization.IRobotController;
import pcsr.hwVirtualization.RobotInfo;
import pcsr.hwVirtualization.Sensor;
import pcsr.hwVirtualization.SensorMode;
import pcsr.hwVirtualization.SensorValue;

public class NXTRobotController implements IRobotController
{
	private static final int ActuatorMaxValue = 100;
	private static final int SensorMaxValue = 4000;

	private Hashtable<String,NXTActuator> actuators;
	
	private NXT controlledNXT;	
	
	private Hashtable<String,NXTSensor> sensors;

	@Override
	public Actuator GetActuatorStatus( Actuator actuator ) {
		// TODO Auto-generated method stub
		NXTActuator theNXTactuator = actuators.get(actuator.GetActuatorID());
		if(theNXTactuator != null)
		{
			theNXTactuator.UpdateActuatorState();
			
			ActuatorMode retactuatormode = new ActuatorMode();
			ArrayList<String> availModes = new ArrayList<String>();
			for(NXTActuatorMode NXTAM : theNXTactuator.GetAvailableModes())
			{
				availModes.add(NXTAM.toString());
			}
			retactuatormode.SetAvailableModes(availModes);
			retactuatormode.SetSelectedMode(theNXTactuator.GetMode().name());
			
			actuator.SetActuatorMode(retactuatormode);
			ActuatorValue retactuatorvalue = new ActuatorValue(ActuatorMaxValue);
			retactuatorvalue.SetValue(theNXTactuator.GetActuatorValue());
			actuator.SetActuatorValue(retactuatorvalue);
			
			return actuator;
		}
		
		return null;
	}
	
	@Override
	public Collection<Actuator> GetAllActuators( ) {
		Collection<Actuator> ret = new LinkedList<Actuator>();
		
		for(NXTActuator na : actuators.values())
		{
			Actuator a = TranslateActuator(na);
			ret.add(a);
		}
		return ret;
	}
	
	@Override
	public Collection<Sensor> GetAllSensors( ) {
		Collection<Sensor> ret = new LinkedList<Sensor>();
		
		for(NXTSensor ns : sensors.values())
		{
			Sensor s = TranslateSensor(ns);
			ret.add(s);
		}
		return ret;
	}

	@Override
	public RobotInfo GetRobotInfo( ) {
		// TODO Auto-generated method stub
		RobotInfo ret = new RobotInfo();
		
		ret.SetName(NXT.GetBrickName());
		
		return ret;
	}

	@Override
	public Sensor GetSensorStatus( Sensor sensor ) {
		// TODO Auto-generated method stub
		NXTSensor theNXTsensor = sensors.get(sensor.GetSensorID());
		if(theNXTsensor != null)
		{
			theNXTsensor.UpdateActuatorState();
			
			SensorMode retsensormode = new SensorMode();
			ArrayList<String> availModes = new ArrayList<String>();
			for(NXTSensorMode NXTSM : theNXTsensor.GetAvailableModes())
			{
				availModes.add(NXTSM.toString());
			}
			retsensormode.SetAvailableModes(availModes);
			retsensormode.SetSelectedMode(theNXTsensor.GetMode().name());
			sensor.SetSensorMode(retsensormode);
			SensorValue retsensorvalue = new SensorValue(SensorMaxValue);
			retsensorvalue.SetValue(theNXTsensor.GetSensorValue());
			sensor.SetSensorValue(retsensorvalue);
			
			return sensor;
		}
		return null;
	}

	/***
	 * Initializes the robot controller.
	 */
	public void Initialize( ) {
		controlledNXT = new NXT();
		
		controlledNXT.Initialize(true);
		
		NXTActuator a = (NXTActuator) controlledNXT.GetActuator_A();
		actuators.put(a.GetPort().name(), a);
		NXTActuator b = (NXTActuator) controlledNXT.GetActuator_B();
		actuators.put(b.GetPort().name(), b);
		NXTActuator c = (NXTActuator) controlledNXT.GetActuator_C();
		actuators.put(c.GetPort().name(), c);
		
		NXTSensor s1 = (NXTSensor) controlledNXT.GetSensor_1();
		sensors.put(s1.GetPort().name(), s1);
		NXTSensor s2 = (NXTSensor) controlledNXT.GetSensor_2();
		sensors.put(s2.GetPort().name(), s2);
		NXTSensor s3 = (NXTSensor) controlledNXT.GetSensor_3();
		sensors.put(s3.GetPort().name(), s3);
		NXTSensor s4 = (NXTSensor) controlledNXT.GetSensor_4();
		sensors.put(s4.GetPort().name(), s4);
	}

	@Override
	public void SetActuatorStatus( Actuator actuator ) {
		// TODO Auto-generated method stub
		
		NXTActuator theNXTactuator = actuators.get(actuator.GetActuatorID());
		if(theNXTactuator != null)
		{			
			theNXTactuator.SetActuatorValue(actuator.GetActuatorValue().GetValue());
			theNXTactuator.SetMode(NXTActuatorMode.valueOf(actuator.GetOperationMode().GetSelectedMode()));
			theNXTactuator.SetActuatorState();
		}
	}

	@Override
	public void SetSensorStatus( Sensor sensor ) {
		// TODO Auto-generated method stub
		NXTSensor theNXTsensor = sensors.get(sensor.GetSensorID());
		if(theNXTsensor != null)
		{			
			theNXTsensor.SetSensorValue(sensor.GetSensorValue().GetValue());
			theNXTsensor.SetMode(NXTSensorMode.valueOf(sensor.GetOperationMode().GetSelectedMode()));
			theNXTsensor.SetSensorState();
		}
	}

	private Actuator TranslateActuator( NXTActuator na ) {
		Actuator a = new Actuator(na.GetPort().name());
		
		ActuatorValue av = new ActuatorValue(ActuatorMaxValue);
		double avv = na.GetActuatorValue();
		av.SetUnnormalizedValue(avv);
		a.SetActuatorValue(av);
		
		ActuatorMode am = new ActuatorMode();
		
		ArrayList<String> availModes = new ArrayList<String>();
		for(NXTActuatorMode NXTAM : na.GetAvailableModes())
		{
			availModes.add(NXTAM.toString());
		}
		am.SetAvailableModes(availModes);
		
		am.SetSelectedMode(na.GetMode().toString());
		a.SetOperationMode(am);
		
		return a;
	}

	private Sensor TranslateSensor( NXTSensor ns ) {
		Sensor s = new Sensor(ns.GetPort().name());
		
		SensorMode sm = new SensorMode();
		
		ArrayList<String> availModes = new ArrayList<String>();
		for(NXTSensorMode NXTSM : ns.GetAvailableModes())
		{
			availModes.add(NXTSM.toString());
		}
		sm.SetAvailableModes(availModes);
		sm.SetSelectedMode(ns.GetMode().toString());	
		s.SetSensorMode(sm);
		
		SensorValue sv = new SensorValue(SensorMaxValue);
		double svv = ns.GetSensorValue();
		sv.SetUnnormalizedValue(svv);
		s.SetSensorValue(sv);
		
		return s;
	}

	{
		actuators = new Hashtable<String, NXTActuator>();
		sensors = new Hashtable<String, NXTSensor>();
	}
	
	@Override
	protected void finalize( ) throws Throwable {
		super.finalize();
		controlledNXT.CloseConnection();
		
		System.err.println("NXT Controler: Connection Closed!");
	}
}
